#include <pronano.h>
#include <md5.h>
#include <Measurement.h>
#include <tft_lcd.h>

volatile uint8_t EndFlag0 = 0, start_I2C,  Date_valid;
uint8_t DataLen0, Tx_Data0[10], Rx_Data0[10];
volatile uint8_t MBI5170_CC_562nm = 0, MBI5170_CC_595nm = 0, MBI5170_CC_660nm = 0;
extern const unsigned char gMain_menu_page[][12], g_Msg_page[][12];

//==============================================================================
//Subroutine: is_RTC_Low_Power
//Function: when the RTC battery volt less than 3.3*low_bat_percent
//==============================================================================
int is_RTC_Low_Power(void)
{
  uint32_t u32ConversionData;
  double volt;

  DrvADC_Open(ADC_SINGLE_END, ADC_SINGLE_OP, 1<<7, EXT_12MHZ, 1);
  DrvADC_StartConvert();
  while(DrvADC_IsConversionDone()==FALSE); 
  u32ConversionData = DrvADC_GetConversionData(7);
  volt = (((double) u32ConversionData)*3.3)/4096;
#ifdef APP_DEBUG
  printf("RTC power: %lf volt\n", volt);
#endif
  DrvADC_Close();

  if (volt < (3.3*0.5))  //low battery volt
    return 1;
  return 0;
}

/*---------------------------------------------------------------------------------------------------------*/
/*  I2C0 (Master) Tx Callback Function, I2C0 as transmitter				                                   */
/*---------------------------------------------------------------------------------------------------------*/
void I2C0_Callback_Tx(uint32_t status)
{
	if (EndFlag0) {
		DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
#ifdef SYS_DEBUG
		printf("Tx EndFlag0: %d\n", EndFlag0);
#endif
		return;
	}

	if (status == 0x08)						/* START has been transmitted */
	{
		if (!start_I2C) {
			DataLen0 = 0;
			DrvI2C_WriteData(I2C_PORT0, M41T0_I2C_Slave_Addr<<1);
			DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
#ifdef SYS_DEBUG
			printf("start I2C Tx\n");
#endif
			start_I2C = 1;
		}
		else {
			EndFlag0 = 1;
			DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
		}
	}	
	else if (status == 0x18)				/* SLA+W has been transmitted and ACK has been received */
	{
		//DrvI2C_WriteData(I2C_PORT0, Tx_Data0[DataLen0++]);
		DrvI2C_WriteData(I2C_PORT0, Tx_Data0[0]);  //word addrsss, 8-bit
		DataLen0++;
		DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
	}
	else if (status == 0x20)				/* SLA+W has been transmitted and NACK has been received */
	{

		DrvI2C_Ctrl(I2C_PORT0, 1, 1, 1, 0);
	}	
	else if (status == 0x28)				/* DATA has been transmitted and ACK has been received */
	{
/*		if (DataLen0 != 3)
		{
			DrvI2C_WriteData(I2C_PORT0, Tx_Data0[DataLen0++]);
			DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
		}
		else
		{
			DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
			EndFlag0 = 1;
		}*/
		if (DataLen0 < Tx_Data0[1]) {
			DrvI2C_WriteData(I2C_PORT0, Tx_Data0[1+DataLen0]);
			DataLen0++;
			DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
		}
		else {
			DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
			EndFlag0 = 1;
		}
	}
	else
	{
#ifdef SYS_DEBUG
		printf("I2C_Tx Status 0x%x is NOT processed\n", status);
#endif
		EndFlag0 = 1;
	}
}


/*---------------------------------------------------------------------------------------------------------*/
/*  I2C0 (Master) Rx Callback Function									                                   */
/*---------------------------------------------------------------------------------------------------------*/
void I2C0_Callback_Rx(uint32_t status)
{
	if (EndFlag0) {
		DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
#ifdef SYS_DEBUG
		printf("Rx EndFlag0: %d\n", EndFlag0);
#endif
	  return;
	}

	if (status == 0x08)					   	/* START has been transmitted and prepare SLA+W */
	{
	  if (!start_I2C) {
        DataLen0 = 0;
		DrvI2C_WriteData(I2C_PORT0, M41T0_I2C_Slave_Addr<<1);
		DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
#ifdef SYS_DEBUG
		printf("start I2C Rx\n");
#endif
		start_I2C = 1;
	  }
	  else {
         EndFlag0 = 1;
		 DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
		 Date_valid = 0;
	  }
	}	
	else if (status == 0x18)				/* SLA+W has been transmitted and ACK has been received */
	{
		//DrvI2C_WriteData(I2C_PORT0, Tx_Data0[DataLen0++]);
		DrvI2C_WriteData(I2C_PORT0, Rx_Data0[0]);  //word addrsss, 8-bit
		DataLen0++;
		DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
	}
	else if (status == 0x20)				/* SLA+W has been transmitted and NACK has been received */
	{
		DrvI2C_Ctrl(I2C_PORT0, 1, 1, 1, 0);
	}
	else if (status == 0x28)				/* DATA has been transmitted and ACK has been received */
	{   //word address has been transmitted
		/*if (DataLen0 != 2)
		{
			DrvI2C_WriteData(I2C_PORT0, Tx_Data0[DataLen0++]);
			DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
		}
		else
		{
			DrvI2C_Ctrl(I2C_PORT0, 1, 0, 1, 0);
		}		*/
		DrvI2C_Ctrl(I2C_PORT0, 1, 0, 1, 0);
	}
	else if (status == 0x10)				/* Repeat START has been transmitted and prepare SLA+R */
	{
		DrvI2C_WriteData(I2C_PORT0, M41T0_I2C_Slave_Addr<<1 | 0x01);
		DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
	}
	else if (status == 0x40)				/* SLA+R has been transmitted and ACK has been received */
	{
		DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
	}		
	else if (status == 0x58)				/* DATA has been received and NACK has been returned */
	{
/*		Rx_Data0 = DrvI2C_ReadData(I2C_PORT0);
		DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
		EndFlag0 = 1;*/
		if (DataLen0 < Rx_Data0[1]) {
		  //for (; DataLen0 < Rx_Data0[1]; DataLen0++)
		     Rx_Data0[1+DataLen0] = DrvI2C_ReadData(I2C_PORT0);
		     DataLen0++;
		  DrvI2C_Ctrl(I2C_PORT0, 0, 0, 1, 0);
		}
		else {
		   DrvI2C_Ctrl(I2C_PORT0, 0, 1, 1, 0);
		   EndFlag0 = 1;
		}
	}
	else
	{
#ifdef SYS_DEBUG
		printf("I2C_Rx Status 0x%x is NOT processed\n", status);
#endif
		EndFlag0 = 1;
		Date_valid = 0;
	}			
}



/*---------------------------------------------------------------------------------------------------------*/
/*  I2C0 (Master) Timeout Function									                                   */
/*---------------------------------------------------------------------------------------------------------*/
void I2C0_Timeout(uint32_t status)
{
#ifdef SYS_DEBUG
  printf("I2C time out\n");
#endif
  EndFlag0 = 1;
}

//==============================================================================
//Subroutine:	Get_Maestro_RTC
//Function: Get date/time from RTC
//Parameters:
//              index  -[in]  date & time field index
//==============================================================================
void Get_Maestro_RTC(union MaestroDate *pDate, int index) 
{
	int i;
	UNLOCKREG();
	Rx_Data0[1] = 2;
	DrvI2C_InstallCallback(I2C_PORT0, I2CFUNC, I2C0_Callback_Rx);
	DrvI2C_InstallCallback(I2C_PORT0, TIMEOUT, I2C0_Timeout);
	DrvI2C_EnableTimeoutCount(I2C_PORT0, 1, 1);

	for (i = 5; i >= 0; i--) {
		EndFlag0 = 0;
		if (i > 2)
			Rx_Data0[0] = i + 1;
		else
			Rx_Data0[0] = i;

		start_I2C = 0;
		EndFlag0 = 0;
		Date_valid = 1;
		DrvI2C_Ctrl(I2C_PORT0, 1, 0, 0, 0);
		while (EndFlag0 == 0);

		if (Date_valid) {
          switch(i) {
	        case 0:
	          pDate->PackDate.Second = 10 * ((Rx_Data0[2]&0x7f) >> 4) + (Rx_Data0[2]&0x0f);
			  break;

			case 1:
				pDate->PackDate.Minute = 10 * ((Rx_Data0[2]&0x7f) >> 4) + (Rx_Data0[2]&0x0f);
				break;
			case 2:
				pDate->PackDate.Hour = 10 * ((Rx_Data0[2]&0x3f) >> 4) + (Rx_Data0[2]&0x0f);
				break;
			case 3:
				pDate->PackDate.Day = 10 * ((Rx_Data0[2]&0x3f) >> 4) + (Rx_Data0[2]&0x0f);
				break;
			case 4:
				pDate->PackDate.Month = 10 * ((Rx_Data0[2]&0x1f) >> 4) + (Rx_Data0[2]&0x0f);
				break;
			case 5:
				pDate->PackDate.Year = 10 * ((Rx_Data0[2]&0xff) >> 4) + (Rx_Data0[2]&0x0f);
				if (pDate->PackDate.Year < 100)
					pDate->PackDate.Year += 2000;
				break;
		  }
		}
	}

	DrvI2C_UninstallCallBack(I2C_PORT0, TIMEOUT);
	DrvI2C_UninstallCallBack(I2C_PORT0, I2CFUNC);
	DrvI2C_EnableTimeoutCount(I2C_PORT0, 0, 0);
	LOCKREG();
	LOCKREG();
}


//==============================================================================
//Subroutine:	Set_Maestro_RTC
//Function: Set date/time from RTC
//Parameters:
//              index  -[in]  date & time field index
//==============================================================================
void Set_Maestro_RTC(union MaestroDate *pDate, int index)
{
//Tx_Data0[2] = pDate->PackDate.Second;
	Tx_Data0[2] = (pDate->PackDate.Second / 10) << 4;
	Tx_Data0[2] += (pDate->PackDate.Second % 10);

//Tx_Data0[3] = pDate->PackDate.Minute;
	Tx_Data0[3] = (pDate->PackDate.Minute / 10) << 4;
	Tx_Data0[3] += (pDate->PackDate.Minute % 10);
	   
//Tx_Data0[4] = pDate->PackDate.Hour;
	Tx_Data0[4] = (pDate->PackDate.Hour / 10) << 4;
	Tx_Data0[4] += (pDate->PackDate.Hour % 10);
	Tx_Data0[5] = 3;
       
//Tx_Data0[6] = pDate->PackDate.Day;
	Tx_Data0[6] = (pDate->PackDate.Day / 10) << 4;
	Tx_Data0[6] += (pDate->PackDate.Day % 10);
       
//Tx_Data0[7] = pDate->PackDate.Month;
	Tx_Data0[7] = (pDate->PackDate.Month / 10) << 4;
	Tx_Data0[7] += (pDate->PackDate.Month % 10);
       
//Tx_Data0[8] = pDate->PackDate.Year;
	if (pDate->PackDate.Year < 100) {
		Tx_Data0[8] = (pDate->PackDate.Year / 10) << 4;
		Tx_Data0[8] += (pDate->PackDate.Year % 10);
	}
	else {
		Tx_Data0[8] = ((pDate->PackDate.Year-2000) / 10) << 4;
		Tx_Data0[8] += ((pDate->PackDate.Year-2000) % 10);	   
	}
	Tx_Data0[1] = 8;
	Tx_Data0[0] = 0;

	switch (index) {
      case 0:
		  Tx_Data0[0] = 0;
		  break;
	  case 1:
		  Tx_Data0[0] = 1;
		  Tx_Data0[2] = Tx_Data0[3];
		  break;
	  case 2:
		  Tx_Data0[0] = 2;
		  Tx_Data0[2] = Tx_Data0[4];
		  break;
	  case 3:
		  Tx_Data0[0] = 4;
		  Tx_Data0[2] = Tx_Data0[6];
		  break;
	  case 4:
		  Tx_Data0[0] = 5;
		  Tx_Data0[2] = Tx_Data0[7];
		  break;
	  case 5:
		  Tx_Data0[0] = 6;
		  Tx_Data0[2] = Tx_Data0[8];
		  break;
	}

	if (index < 6) {
		Tx_Data0[1] = 2;
	}
}


//==============================================================================
//Subroutine: Update_Sheet_Index
//Function: Update sheet index to data flash
//==============================================================================
void Update_sheet_index(int cur_sheet, int head_sheet, int tail_sheet)  //default, cur_sheet=0, head_sheet = -1, tail_sheet = -1
{
  uint32_t u32Data;

  UNLOCKREG();
#ifdef USE_BSP_ver_June_2011
  DrvFMC_EnableISP();
#else
  DrvFMC_EnableISP(1);
#endif //USE_BSP_ver_June_2011
  DrvFMC_Read(0x1FFF0, &u32Data);
  if (u32Data==VersionID) {  //first use data flash
	DrvFMC_Erase(0x1FE00);
    DrvFMC_Write(0x1FFFC, tail_sheet);
	DrvFMC_Write(0x1FFF8, head_sheet);
	DrvFMC_Write(0x1FFF4, cur_sheet);
	DrvFMC_Write(0x1FFF0, VersionID);
  }
#ifdef USE_BSP_ver_June_2011
  DrvFMC_DisableISP();
#else
  DrvFMC_EnableISP(0);
#endif //USE_BSP_ver_June_2011
  
  LOCKREG();
}


//==============================================================================
//Subroutine: Store_sheet
//Function: Store current sheet into flash
//==============================================================================
int Store_sheet(struct Measure_tran *pMea_tran, int type, struct Measure_sheet *pSheet, int sheet_index) //type: 0,blank; type: 1,BCA; type: 2,Bradford; type: 3,Pierce
{
   int32_t i32Err = 0;
   uint32_t u32Data, i, pages, *pData;
   struct PackedDate *pPackdate;
   //struct ProNano_Flash_t flash_data;
   struct MD5Context md5c;
   unsigned char digest[16];

   pSheet->sheet_type = type;
   if (!is_RTC_Low_Power())
	   Get_Maestro_RTC(&pSheet->sheet_date, 5);

// flush pMea_tran into flash
   printf("store sheet_index: %d\n", sheet_index)	;
   UNLOCKREG();
#ifdef USE_BSP_ver_June_2011
   DrvFMC_EnableISP();
#else
   DrvFMC_EnableISP(1);
#endif

   pMea_tran->sheet_type = pSheet->sheet_type;
   pMea_tran->sheet_date = pSheet->sheet_date;

   /* Initialize MD5 module: */                                       
   MD5Init(&md5c);
   MD5Update (&md5c, (char *)pMea_tran, sizeof(struct Measure_tran)-8);
   MD5Final (digest, &md5c);
   strncpy(pMea_tran->check_digits, digest, 8);

   /* Page Erase LDROM */
   if (sizeof(struct Measure_tran) % DATA_FLASH_PAGESIZE)
	   pages = sizeof(struct Measure_tran) / DATA_FLASH_PAGESIZE + 1;
   else
	   pages = sizeof(struct Measure_tran) / DATA_FLASH_PAGESIZE;

   for (i=0; i < pages; i++) {
	   u32Data = DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages+i);
	   if (u32Data < FLASH_SIZE)
		   DrvFMC_Erase(u32Data);
	   else
		   printf("Flash erase address invalid: %x\n", u32Data);
   }

   pData = (uint32_t *)pMea_tran;
   for (i = DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages); i < (DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages)+sizeof(struct Measure_tran)); i+=4, pData++) {
	   DrvFMC_Write(i, *pData);
   }
   pData = (uint32_t *)pMea_tran;
   i32Err = 0;
   for (i = DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages); i < (DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages)+sizeof(struct Measure_tran)); i+=4, pData++) {
	   DrvFMC_Read(i, &u32Data);
	   //     *pData = u32Data;
	   if (u32Data != *pData)
	   { 
		   i32Err = 1;
	   }
   }
   if (i32Err) {
	   printf("Store pSheet xxx [FAIL]\n");
	   return -1;
   }
   else
	   printf("Store pSheet xxx1 [OK]\n");

   LOCKREG();
   return 0;
}


//==============================================================================
//Subroutine: Load_sheet
//Function: Load current sheet from flash
//==============================================================================
int Load_sheet(struct Measure_tran *pMea_tran, struct Measure_sheet *pSheet, int sheet_index) //type: 0,blank; type: 1,BCA; type: 2,Bradford; type: 3,Pierce
{
#ifdef USE_DATA_FLASH
	int32_t i32Err = 0;
	uint32_t u32Data, i, pages, *pData;
	struct PackedDate *pPackdate;
   //struct MaestroNano_Flash_t flash_data;
   struct MD5Context md5c;
   unsigned char digest[16];

   printf("load sheet_index: %d, MaestroNano_Flash_t: %d\n", sheet_index, sizeof(struct Measure_tran))	;
   UNLOCKREG();
#ifdef USE_BSP_ver_June_2011
   DrvFMC_EnableISP();
#else
   DrvFMC_EnableISP(1);
#endif

//#ifdef MaestroNano_Algo
   pData = (uint32_t *)pMea_tran;
   i32Err = 0;
   if (sizeof(struct Measure_tran) % DATA_FLASH_PAGESIZE)
	   pages = sizeof(struct Measure_tran) / DATA_FLASH_PAGESIZE + 1;
   else
	   pages = sizeof(struct Measure_tran) / DATA_FLASH_PAGESIZE;
   for (i = DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages); i < (DATA_FLASH_BASE+DATA_FLASH_PAGESIZE*(sheet_index*pages)+sizeof(struct Measure_tran)); i+=4, pData++) {
	   DrvFMC_Read(i, &u32Data);
	   *pData = u32Data;
	   if (u32Data != *pData)
	   { 
		   i32Err = 1;
	   }
   }
   MD5Init(&md5c);
   MD5Update (&md5c, (char *)pMea_tran, sizeof(struct Measure_tran)-8);
   MD5Final (digest, &md5c);

   if (!strncmp(pMea_tran->check_digits, digest, 8)) {
	   pSheet->sheet_type = pMea_tran->sheet_type;
	   pSheet->sheet_date = pMea_tran->sheet_date;
	   printf("Load sheet [OK]\n");
	   return 0;
   }
   else {
	   printf("Load sheet [FAIL]\n");
	   return -1;
   }
//#else
//#endif

   LOCKREG();
#endif
}

//==============================================================================
//Subroutine: Print_Selected
//Function: only print selected measure data in the current sheet
//==============================================================================
#ifdef PRINTER_PORT
int Print_Selected(struct Measure_tran *pMea_tran, int *pIndex, struct Measure_sheet *pSheet) {
  int i = 0, columns = 0;
  uint8_t pu8CTSValue = 1, pu8CTSChangeState = 0;
  
//reset printer, "ESC @"=="1B 40"
  //buf2[0] = 0x1b; buf2[1] = 0x40;
  //DrvUART_Write(PRINTER_PORT, buf2, 2);

  bPrinter_status = 0;
  bPrinter_error = 0;
  bPrinter_warning = 0;
//check printer status, "ESC v"=="1B 76"
  buf2[0] = 0x1b; buf2[1] = 0x76;
  DrvUART_Write(PRINTER_PORT, buf2, 2);
  Delay100ms(2);

  if (bPrinter_status!=0x00) {
	  //status bit5
	  if (!(bPrinter_status&(1 << 5)))  //0. printer off-line
	  {
		  bPrinter_error |= 1;
		  //printf("printer status 1 error\n");
	  }
	  //else
	  //printf("printer unknown error");
	  //status bit1
	  if ((bPrinter_status&(1 << 1)))  //2. Platen opened, head up
		  bPrinter_error |= (1 << 1);
	  //status bit2
	  if (bPrinter_status&(1 << 2))  //3. printing interrupt by paper shortage
		  bPrinter_error |= (1 << 2);

	  //shingya thermal printer with 5 errors
	  //status bit0
	  if ((bPrinter_status&0x01))  //4. Head Temperature too high or too low
		  bPrinter_error |= (1 << 3);
	  //status bit3
	  if ((bPrinter_status&(1 << 3)))  //5. Power supply too high or too low
		  bPrinter_error |= (1 << 4);
	  //status bit7
	  if (!(bPrinter_status&(1 << 7)))  //6. Cutter error
		  bPrinter_error |= (1 << 3);
  }

  if (bPrinter_error) {
	  return bPrinter_error;
  }

//near end of paper status, "ESC n s"="1B 6E 73"
  buf2[0] = 0x1b; buf2[1] = 0x6e; buf2[2] = 0x73;
  DrvUART_Write(PRINTER_PORT, buf2, 3);
  Delay100ms(2);
  if (bPrinter_status==0x01) {
  	  TFT_Send_Command(gMain_menu_page[0], 12, 0);  //background
	  TFT_Send_Command(g_Msg_page[68], 12, 0);  //paper near end
	  Delay100ms(35);
	  bPrinter_warning |= 1;
  }

// init printer
	/*buf2[0] = 0x1B;
	buf2[1] = 0x40;
	buf2[2] = 0;
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));*/

#if 1
#ifdef Printer_test
  /*bPrinter_status = 0;
  bPrinter_error = 0;
  bPrinter_warning = 0;*/

//check printer status, "ESC v"=="1B 76"
  /*buf2[0] = 0x1b; buf2[1] = 0x76;
  DrvUART_Write(PRINTER_PORT, buf2, 2);
  Delay100ms(20);*/

/*20130611 added by michael
font size, "ESC % n"=="1B 25 2"*/
  buf2[0] = 0x1b; buf2[1] = 0x25; buf2[2] = 0x02;
  DrvUART_Write(PRINTER_PORT, buf2, 3);

  strcpy(buf2, "================BCA================\n\r");
/*20130611 added by michael
Set maximum number of columns, "ESC c n"="1B 63 n"*/
  /*columns = strlen(buf2);
  buf2[0] = 0x1b; buf2[1] = 0x63; buf2[2] = columns;
  DrvUART_Write(PRINTER_PORT, buf2, 3);*/

/*20130611 added by michael
intensity level n > 0x80 is darker, n < 0x80 is lighter
set print mode, "ESC ! n"="1B 21 n"*/
  buf2[0] = 0x1d; buf2[1] = 0x44; buf2[2] = 190;
  DrvUART_Write(PRINTER_PORT, buf2, 3);

/*20130611 added by michael
Set character spacing, "ESC SP n"="1B 20 n"*/
  buf2[0] = 0x1b; buf2[1] = 0x20; buf2[2] = 0x00;
  DrvUART_Write(PRINTER_PORT, buf2, 3);

/*set intensity*/
//GS 'D' 190
  buf2[0] = 0x1b; buf2[1] = 0x20; buf2[2] = 0x00;
  DrvUART_Write(PRINTER_PORT, buf2, 3);

/*20130611 added by michael
Set line spacing, "ESC 3 n"=="1B 33 n"*/
  buf2[0] = 0x1b; buf2[1] = 0x33; buf2[2] = 0x08;
  DrvUART_Write(PRINTER_PORT, buf2, 3);

/*20130611 added by michael
Set maximum printing speed, "GS s n1 n2"="1D 73 n1 n2"*/
  buf2[0] = 0x1d; buf2[1] = 0x73; buf2[2] = 0x01; buf2[3] = 226;
  DrvUART_Write(PRINTER_PORT, buf2, 4);

  /*strcpy(buf2, "================BCA================\n\r");
  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
  sprintf(buf2, " Date %04d/%02d/%02d\n\r", pSheet->sheet_date.PackDate.Year, pSheet->sheet_date.PackDate.Month, pSheet->sheet_date.PackDate.Day);
  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
  sprintf(buf2, " Time %02d:%02d:%02d\n\r", pSheet->sheet_date.PackDate.Hour, pSheet->sheet_date.PackDate.Minute, pSheet->sheet_date.PackDate.Second);
  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));

  strcpy(buf2, "\n\r\n\r\n\r\n\r\n\r");
  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
  buf2[0] = 0x1b; buf2[1] = 0x69; buf2[2] = 0x00;
  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));*/
  //return 0;
  
#endif
#endif

//intensity level n > 0x80 is darker, n < 0x80 is lighter
  //buf2[0] = 0x1d; buf2[1] = 0x44; buf2[2] = 190;
  //DrvUART_Write(PRINTER_PORT, buf2, 3);

//serial port setting, bit7=1 to select RTS/CTS or DSR/DTR hardware handshaking; bit7=0 to choice software handshaking
  //buf2[0] = 0x1d; buf2[1] = 0x42; buf2[2] = 0x83;
  //DrvUART_Write(PRINTER_PORT, buf2, 3);

//Cancel Kanji character mode
	/*buf2[0] = 0x1C;
	buf2[1] = 0x2E;
	buf2[2] = 0;
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));*/

//Select print mode(s), "ESC ! n"=="1B 21 n"
	/*buf2[0] = 0x1B;
	buf2[1] = 0x21;
	buf2[2] = 0x00;
	buf2[3] = 0;
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));*/

	if (pSheet && pIndex && pMea_tran) {
		switch(pSheet->sheet_type) {
			case 1:
				strcpy(buf2, "===============BCA==============\n\r");
				break;
			case 2:
				strcpy(buf2, "============Bradford============\n\r");
				break;
			case 3:
				strcpy(buf2, "=============Pierce=============\n\r");
				break;
		}

		WAIT_CTS_ASSERT
		DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
#ifdef LOCCUS_ODM
		sprintf(buf2, " Data %02d/%02d/%04d\n\r", pSheet->sheet_date.PackDate.Day, pSheet->sheet_date.PackDate.Month, pSheet->sheet_date.PackDate.Year);
#else
		sprintf(buf2, " Date %04d/%02d/%02d\n\r", pSheet->sheet_date.PackDate.Year, pSheet->sheet_date.PackDate.Month, pSheet->sheet_date.PackDate.Day);
#endif
        WAIT_CTS_ASSERT
		DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
#ifdef LOCCUS_ODM
		sprintf(buf2, " Hora %02d:%02d:%02d\n\r", pSheet->sheet_date.PackDate.Hour, pSheet->sheet_date.PackDate.Minute, pSheet->sheet_date.PackDate.Second);
#else
		sprintf(buf2, " Time %02d:%02d:%02d\n\r", pSheet->sheet_date.PackDate.Hour, pSheet->sheet_date.PackDate.Minute, pSheet->sheet_date.PackDate.Second);
#endif
        WAIT_CTS_ASSERT
		DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
		sprintf(buf2, " ===============================\n\r");
		WAIT_CTS_ASSERT
		DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));

		for (i = 0; i < (MAX_Samples+1); i++) {
			if (!i && pIndex[i]==1) {
#ifdef LOCCUS_ODM
				sprintf(buf2, "\n\r   Branco  %02d/%02d/%04d %02d:%02d:%02d\n\r\n\r", pMea_tran->date[0].PackDate.Day, pMea_tran->date[0].PackDate.Month, pMea_tran->date[0].PackDate.Year, pMea_tran->date[0].PackDate.Hour, pMea_tran->date[0].PackDate.Minute, pMea_tran->date[0].PackDate.Second);
#else
				sprintf(buf2, "\n\r   blank  %04d/%02d/%02d %02d:%02d:%02d\n\r\n\r", pMea_tran->date[0].PackDate.Year, pMea_tran->date[0].PackDate.Month, pMea_tran->date[0].PackDate.Day, pMea_tran->date[0].PackDate.Hour, pMea_tran->date[0].PackDate.Minute, pMea_tran->date[0].PackDate.Second);
#endif
                WAIT_CTS_ASSERT
				DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
			}
			else
				if (i && pIndex[i]==1) {
#ifdef LOCCUS_ODM
					sprintf(buf2, "   Amostra_%d:\n\r", pMea_tran->sample_No[i-1]);
#else
					sprintf(buf2, "   sample_%d:", pMea_tran->sample_No[i-1]);
#endif
					WAIT_CTS_ASSERT
					DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
					sprintf(buf2, "\n\r");
					WAIT_CTS_ASSERT
					DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));                 
					//Delay100ms(2);
					/*buf2[0] = 0x1B;
					buf2[1] = 0x21;
					buf2[2] = 0x01;
					buf2[3] = 0;
					DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));*/

					switch(pSheet->sheet_type) {
				      case 1:
						  strcpy(buf2, "   A562");
						  WAIT_CTS_ASSERT
						  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  sprintf(buf2, "    %3.3f", 1000*pMea_tran->AXXX[i-1]);
						  WAIT_CTS_ASSERT
						  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  sprintf(buf2, "\n\r\n\r");
						  WAIT_CTS_ASSERT
					      DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  break;

					  case 2:
						  strcpy(buf2, "   A595");
						  WAIT_CTS_ASSERT
						  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  sprintf(buf2, "    %3.3f", 1000*pMea_tran->AXXX[i-1]);
						  WAIT_CTS_ASSERT
						  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  sprintf(buf2, "\n\r\n\r");
						  WAIT_CTS_ASSERT
					      DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  break;

					  case 3:
						  strcpy(buf2, "   A660");
						  WAIT_CTS_ASSERT
						  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  sprintf(buf2, "    %4.3f", 10000*pMea_tran->AXXX[i-1]);
						  WAIT_CTS_ASSERT
						  DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  sprintf(buf2, "\n\r\n\r");
						  WAIT_CTS_ASSERT
					      DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
						  break;

					}
				}
		}
	}
	else
		printf("pSheet=%x  pIndex=%x  pMea_tran=%x", pSheet, pIndex, pMea_tran);

 
    //Delay100ms(2);
	sprintf(buf2, "\n\r\n\r\n\r");
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	/*WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));*/
//cut paper, "ESC i"=="1B 69"
	buf2[0] = 0x1b; buf2[1] = 0x69; buf2[2] = 0x00;
	WAIT_CTS_ASSERT
	DrvUART_Write(PRINTER_PORT, buf2, strlen(buf2));
	return bPrinter_error;
}

#else

int Print_Selected(struct Measure_tran *pMea_tran, int *pIndex, struct Measure_sheet *pSheet) {
	printf("sheet_type: %d\n", pSheet->sheet_type);
}

#endif  //PRINTER_PORT


//==============================================================================
//Subroutine: MBI5170_Normal_Mode
//Function: MBI5170 enter normal mode
//==============================================================================
void MBI5170_Normal_Mode()
{
/*20130516 added by michael
refer to MBI5170 data saheet page 17*/
	
	DrvGPIO_SetBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
	//SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);

	
	DrvGPIO_ClrBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
	//SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);

	DrvGPIO_SetBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
	//SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);

	DrvGPIO_ClrBit(GPA, Led_driver_MOD);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
//	SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);
}


//==============================================================================
//Subroutine: MBI5170_Current_Adj_Mode
//Function: MBI5170 enter current adjust mode
//==============================================================================
void MBI5170_Current_Adj_Mode()
{
/*20130516 added by michael
refer to MBI5170 data saheet page 17*/
	
	DrvGPIO_SetBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
//	SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);

	
	DrvGPIO_ClrBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
//	SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);

	DrvGPIO_SetBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
//	SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);

	DrvGPIO_SetBit(GPA, Led_driver_MOD);
	DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	DrvGPIO_SetBit(GPA, Led_driver_CLK);
//	SysTimerDelay(1);
    DrvGPIO_ClrBit(GPA, Led_driver_CLK);
}


//==============================================================================
//Subroutine: Led_Current_Sink_On
//Function: turn on Led negative terminal
//Param:
//         LED_index[in]: 1 for Led 660nm, 2 for Led 595nm, 3 for Led 562nm
//==============================================================================
void Led_Current_Sink_On(int LED_index)
{
    int i;

	MBI5170_Normal_Mode();

	for (i = 0; i < 24; i++) {
        //if ((LED_index*8-1)==i)
		//if ((LED_index*8)==i)
		//if ((LED_index*8-1)==7 || (LED_index*8-1)==15 || (LED_index*8-1)==23)
		if ((LED_index*8-1)==i)
			DrvGPIO_SetBit(GPA, Led_driver_SDI);
		else
			DrvGPIO_ClrBit(GPA, Led_driver_SDI);

		DrvGPIO_SetBit(GPA, Led_driver_CLK);
//		SysTimerDelay(1);
		DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	}


	DrvGPIO_SetBit(GPA, Led_driver_MOD);
//	SysTimerDelay(1);
	DrvGPIO_ClrBit(GPA, Led_driver_MOD);
	DrvGPIO_ClrBit(GPA, Led_driver_OE);
	//Delay100ms(1);
	//DrvGPIO_SetBit(GPA, Led_driver_OE);
}

//==============================================================================
//Subroutine: Led_Current_Sink_Off
//Function: turn on Led negative terminal
//==============================================================================
void Led_Current_Sink_Off()
{   
#if 1
	int i;
    MBI5170_Normal_Mode();

	DrvGPIO_ClrBit(GPA, Led_driver_SDI);
	for (i = 0; i < 24; i++) {
		DrvGPIO_SetBit(GPA, Led_driver_CLK);
//		SysTimerDelay(1);
		DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	}

	DrvGPIO_SetBit(GPA, Led_driver_MOD);
//	SysTimerDelay(1);
	DrvGPIO_ClrBit(GPA, Led_driver_MOD);
	DrvGPIO_ClrBit(GPA, Led_driver_OE);
//	SysTimerDelay(1);
#endif
	DrvGPIO_SetBit(GPA, Led_driver_OE);
	DrvGPIO_ClrBit(GPA, Led_driver_SDI);
}

//==============================================================================
//Subroutine: MBI5170_Set_Current
//Function: write current configuration code into MBI5170
//Param:
//         LED_index[in]: 1 for Led 660nm, 2 for Led 595nm, 3 for Led 562nm
//==============================================================================
void MBI5170_Set_Current(int CC, int LED_index)
{
	int i, new_CC = 0;

	MBI5170_Current_Adj_Mode();
	//CC = CC & 0xff;
	//CC |= (CC & 0xff) << 8;

	if (LED_index > 0) {
#if 0
		new_CC = MBI5170_CC_562nm << 16;
		new_CC |= MBI5170_CC_595nm << 8;
		new_CC |= MBI5170_CC_660nm;
#endif
		new_CC |= (CC & 0xff) << ((LED_index-1)*8);

#if 0
		switch (LED_index) {
		  case 1:
			  new_CC = MBI5170_CC_562nm << 16;
			  new_CC |= MBI5170_CC_595nm << 8;
			  break;
		  case 2:
			  new_CC = MBI5170_CC_562nm << 16;
			  new_CC |= MBI5170_CC_660nm;
			  break;
		  case 3:
			  new_CC |= MBI5170_CC_595nm << 8;
			  new_CC |= MBI5170_CC_660nm;
			  break;
		}
#endif
	}
	else
		new_CC = CC;

	for (i = 0; i < 24; i++) {
		if ((new_CC >> i) & 0x01) {
			DrvGPIO_SetBit(GPA, Led_driver_SDI);
		}
		else {
			DrvGPIO_ClrBit(GPA, Led_driver_SDI);
		}
		DrvGPIO_SetBit(GPA, Led_driver_CLK);
//		SysTimerDelay(1);
		DrvGPIO_ClrBit(GPA, Led_driver_CLK);
	}

	DrvGPIO_SetBit(GPA, Led_driver_MOD);
//	SysTimerDelay(1);
	DrvGPIO_ClrBit(GPA, Led_driver_MOD);
	DrvGPIO_ClrBit(GPA, Led_driver_OE);
	//SysTimerDelay(1);
	//DrvGPIO_SetBit(GPA, Led_driver_OE);

	//MBI5170_Normal_Mode();
}

//==============================================================================
//Subroutine: Led current test
//Function: Led 562nm, 595nm, 660nm current test
//==============================================================================
void Led_Current_Test()
{
     //Led_Current_Sink_On(3);
     //Led_Current_Sink_Off();
	 Assay_index = 1;
     LED595_ON
/*20130522 added by michael
MBI5170 current  configuration code divide into four segment
0, 128
*/
	 MBI5170_Set_Current(103, 2);
	 Led_Current_Sink_On(2);
	 LED595_OFF
	 Led_Current_Sink_Off();
	 return;

     //MBI5170_Set_Current(255);
	 ProNano_Measure();
	 Delay100ms(3);

     MBI5170_Set_Current(200, 3);
	 while (1);
	 //ProNano_Measure();
	 Delay100ms(3);

     MBI5170_Set_Current(150, 3);
	 //ProNano_Measure();
	 Delay100ms(3);

     MBI5170_Set_Current(100, 3);
	 //ProNano_Measure();
	 Delay100ms(3);

     MBI5170_Set_Current(50, 3);
	 //ProNano_Measure();
	 Delay100ms(3);

/*	 MBI5170_Set_Current(0);
	 ProNano_Measure();
	 Delay100ms(3);*/
}


//==============================================================================
//Subroutine: Setting_up_CC
//Function: find the most adaptive MBI5170 cofiguration code
//Param:
//         index[in]:  Assay index
//==============================================================================
void Setting_up_CC(int index, int lower_CC, int upper_CC)
{

	int adaptive_CC = 0, j = 0, Max_intensity_diff, target_intensity = 0;

	LED562_OFF
	LED595_OFF
	LED660_OFF


	cur_phase = 1;
	//if (Assay_index==-1) {
		//for (i = 0; i < 3; i++) {
			if (index==0)
				target_intensity = I_Led562_target;
			else
				if (index==1)
					target_intensity = I_Led595_target;
				else
					target_intensity = I_Led660_target;

			Max_intensity_diff = 1040000;
			//upper_CC = 255;
			if (lower_CC < 0)
			  lower_CC = 0;
			adaptive_CC = lower_CC;
			while(upper_CC > lower_CC) { 
				printf("\n\n\nsearching CC in range (%d, %d)\n", lower_CC, upper_CC);
				Led_Off();
				MBI5170_Set_Current((upper_CC+lower_CC)/2, (3-index));

				if (index==0) {
					LED562_ON
					Led_On(3);
				}
				else
					if (index==1) {
						LED595_ON
						Led_On(2);
					}
					else {
						LED660_ON
						Led_On(1);
					}

				MaestroNano_Capture(1, 1);

				if (adc_data[1][0] < target_intensity) {
					lower_CC = (upper_CC+lower_CC)/2;
					if (abs(adc_data[1][0]-target_intensity) < Max_intensity_diff) {
						Max_intensity_diff = abs(adc_data[1][0]-target_intensity);
						adaptive_CC = lower_CC;
					}
#ifdef MBI5170_DEBUG
					printf("#cur_phase: %d, adaptive_CC: %d, intensity: %d\n", cur_phase, adaptive_CC, adc_data[1][0]);
#endif
				}
				else {
					upper_CC = (upper_CC+lower_CC)/2;
					if (abs(adc_data[1][0]-target_intensity) < Max_intensity_diff) {
						Max_intensity_diff = abs(adc_data[1][0]-target_intensity);
						adaptive_CC = upper_CC;
					}
#ifdef MBI5170_DEBUG
					printf("*cur_phase: %d, adaptive_CC: %d, intensity: %d\n", cur_phase, adaptive_CC, adc_data[1][0]);
#endif
				}


				if ((upper_CC-lower_CC)==0 || (upper_CC-lower_CC)==1)
					break;
			}

			if (upper_CC==lower_CC) {
				if (index==0)
					MBI5170_CC_562nm = upper_CC;
				else
					if (index==1)
						MBI5170_CC_595nm = upper_CC;
					else
						MBI5170_CC_660nm = upper_CC;
			}
			else {
				if (index==0)
					MBI5170_CC_562nm = adaptive_CC;
				else
					if (index==1)
						MBI5170_CC_595nm = adaptive_CC;
					else
						MBI5170_CC_660nm = adaptive_CC;			
			}

#ifdef MBI5170_DEBUG
			if (index==0)
				printf("Adaptive CC for 562nm: %d, Max_intensity_diff: %d\n", MBI5170_CC_562nm, Max_intensity_diff);
			else
				if (index==1)
					printf("Adaptive CC for 595nm: %d, Max_intensity_diff: %d\n", MBI5170_CC_595nm, Max_intensity_diff);
				else
					printf("Adaptive CC for 660nm: %d, Max_intensity_diff: %d\n", MBI5170_CC_660nm, Max_intensity_diff);
#endif
			Led_Off();
			MBI5170_Set_Current(upper_CC, (3-index));

#if 0
			for (Max_intensity_diff = 1040000, adaptive_CC = 255; adaptive_CC >= 0; adaptive_CC--) {
				Led_Off();
				MBI5170_Set_Current(adaptive_CC, (3-i));

				if (i==0) {                     
					LED562_ON
						Led_On(3);
					MaestroNano_Capture(1, 1);
#ifdef MBI5170_DEBUG
					printf("cur_phase: %d, adaptive_CC: %d, intensity: %d\n", cur_phase, adaptive_CC, adc_data[1][0]);
#endif
					if (abs(adc_data[1][0] - I_Led562_target) < Max_intensity_diff) {
						MBI5170_CC_562nm = adaptive_CC;
						Max_intensity_diff = abs(adc_data[1][0] - I_Led562_target);
					}
					if (adc_data[1][0] < I_Led562_target) {
						break;
					}
				}

			}

			if (i==0) {
#ifdef MBI5170_DEBUG
				printf("Adaptive CC for 562nm: %d, Max_intensity_diff: %d\n", MBI5170_CC_562nm, Max_intensity_diff);
#endif
				Led_Current_Sink_Off();
				MBI5170_Set_Current(MBI5170_CC_562nm, (3-i));
			}
#endif
		//}
	//}

	printf("final CC:  %d  %d  %d\n\n\n", MBI5170_CC_562nm, MBI5170_CC_595nm, MBI5170_CC_660nm);
	Led_Off();
	MBI5170_Set_Current((MBI5170_CC_562nm << 16) | (MBI5170_CC_595nm << 8) | (MBI5170_CC_660nm), 0);

#if 0
	if (Assay_index==0)  //562nm
	{
		if (MBI5170_CC_562nm==0) {  //do full search CC
			for (Max_intensity_diff = 1040000, adaptive_CC = 255; adaptive_CC >= 0; adaptive_CC--)
			{
				Led_Current_Sink_Off();
				MBI5170_Set_Current(adaptive_CC, (3-Assay_index));
#if 1
				switch (Assay_index) {
	              case 0:
		             LED562_ON
			         Led_On(3);
		             break;
				  case 1:
					  LED595_ON
						  Led_On(2);
					  break;
				  case 2:
					  LED660_ON
						  Led_On(1);
					  break;
				}
#endif
				
				MaestroNano_Capture(1, 1);
				printf("cur_phase: %d, MBI5170_CC_562nm: %d, intensity: %d\n", cur_phase, adaptive_CC, adc_data[1][0]);
				if (abs(adc_data[1][0] - I_Led562_target) < Max_intensity_diff) {
					MBI5170_CC_562nm = adaptive_CC;
                    Max_intensity_diff = abs(adc_data[1][0] - I_Led562_target);
				}
				if (adc_data[1][0] < I_Led562_target)
					break;
			}
			printf("Adaptive CC for 562nm: %d, Max_intensity_diff: %d\n", MBI5170_CC_562nm, Max_intensity_diff);
			Led_Current_Sink_Off();
			MBI5170_Set_Current(208, (3-Assay_index));
		}
		else {  //only search (CC-5) to (CC+5)
		}
	}
	else
		if (Assay_index==1)  //595nm
		{
		
		}
		else {  //660nm
		
		}
#endif

  switch (Assay_index) {
     case 0:
	    LED562_OFF
        break;
	 case 1:
		LED595_OFF
        break;
	 case 2:
		LED660_OFF
        break;
  }
}